Skip to content

Commit

Permalink
Restore G61/G61.1/G64 state after abort
Browse files Browse the repository at this point in the history
After abort, restore motion control mode and blend/naive CAM
tolerances to state of last executing motion before the abort.  This
depends on Rob Ellenberg's state tags work.

The `state_tag_t` struct keeps integer settings in an array, indexed
by the `StateField` enum.  Introducing the `G64 P* Q*` tolerances adds
two more `float` types to the existing two for feed and speed.  This
patch packs those all into a new array of floats, indexed by a new
enum, to simplify handling.  These new floats are exposed in the
python module `status.settings` attribute, positions 3 and 4.

Restoring this state has two parts.  The motion control mode
G61/G61.1/G64 would have been handled by `Interp::gen_g_codes()`, and
the G64 blend/naive CAM tolerances would have been handled by
`Interp::gen_settings()`.  However, a switch from G61/G61.1 to G64 and
a simultaneous switch of P/Q tolerance values cannot be done with two
independent functions.  Therefore, `gen_g_codes()` has been merged
into `gen_settings()` in order to handle this special case.

To keep the tolerances updated during program execution, they are
added to the `Interp::synch()` function.

To restore the naive CAM tolerance, a new function
`GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE()` is introduced,
parallel to `GET_EXTERNAL_MOTION_CONTROL_TOLERANCE()` for blend
tolerance.

This patch fixes #177, "Forgetting G64 mode after E-stop," and one of
the remaining issues in #134, "Updating `state-tags-master` branch for
acceptance into LinuxCNC."

Signed-off-by: John Morris <john@zultron.com>
  • Loading branch information
zultron authored and andypugh committed Apr 19, 2020
1 parent 3bb1439 commit 6bc4a2a
Show file tree
Hide file tree
Showing 18 changed files with 182 additions and 75 deletions.
4 changes: 3 additions & 1 deletion docs/src/config/python-interface.txt
Expand Up @@ -336,7 +336,9 @@ current XY rotation angle around Z axis.

*settings*:: '(returns tuple of floats)' -
current interpreter settings. settings[0] =
sequence number, settings[1] = feed rate, settings[2] = speed.
sequence number, settings[1] = feed rate, settings[2] = speed,
settings[3] = `G64 P` blend tolerance,
settings[4] = `G64 Q` naive CAM tolerance.

*spindle*:: ' (returns tuple of dicts) ' -
returns the current spindle status
Expand Down
22 changes: 18 additions & 4 deletions src/emc/motion/state_tag.h
Expand Up @@ -62,7 +62,7 @@ typedef enum {


/**
* Enum for various fields of state info that aren't binary.
* Enum for various fields of state info that are int type.
*
* WARNING:
*
Expand All @@ -87,6 +87,20 @@ typedef enum {
GM_FIELD_MAX_FIELDS
} StateField;


/**
* Enum for indexing state tag `fields_float`, machine state float
* array: feed, speed, etc.
*/
typedef enum {
GM_FIELD_FLOAT_LINE_NUMBER, // eww
GM_FIELD_FLOAT_FEED,
GM_FIELD_FLOAT_SPEED,
GM_FIELD_FLOAT_PATH_TOLERANCE,
GM_FIELD_FLOAT_NAIVE_CAM_TOLERANCE,
GM_FIELD_FLOAT_MAX_FIELDS
} StateFieldFloat;

/**
* Tag structure that is added to a motion segment so that motion has a copy of
* the relevant interp state.
Expand All @@ -99,9 +113,9 @@ typedef enum {
*/
struct state_tag_t {

// Machine settings
float feed;
float speed;
// Float-type machine settings: feed, speed, etc., indexed by the
// StateFieldFloat enum above
float fields_float[GM_FIELD_FLOAT_MAX_FIELDS];

// Any G / M code states that doesn't pack nicely into a single bit
// These are an array mostly because it's easier to pass an
Expand Down
3 changes: 3 additions & 0 deletions src/emc/nml_intf/canon.hh
Expand Up @@ -794,6 +794,9 @@ extern CANON_MOTION_MODE GET_EXTERNAL_MOTION_CONTROL_MODE();
// Returns the current motion path-following tolerance
extern double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE();

// Returns the current motion naive CAM tolerance
extern double GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE();

/* The interpreter is not using these six GET_EXTERNAL_ORIGIN functions
// returns the current a-axis origin offset
Expand Down
2 changes: 2 additions & 0 deletions src/emc/rs274ngc/canonmodule.cc
Expand Up @@ -136,6 +136,8 @@ BOOST_PYTHON_MODULE(emccanon) {
def("GET_EXTERNAL_MOTION_CONTROL_MODE",&GET_EXTERNAL_MOTION_CONTROL_MODE);
def("GET_EXTERNAL_MOTION_CONTROL_TOLERANCE",&GET_EXTERNAL_MOTION_CONTROL_TOLERANCE);
def("SET_PARAMETER_FILE_NAME",&SET_PARAMETER_FILE_NAME);
def("GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE",
&GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE);
def("GET_EXTERNAL_PARAMETER_FILE_NAME",&GET_EXTERNAL_PARAMETER_FILE_NAME);
def("GET_EXTERNAL_PLANE",&GET_EXTERNAL_PLANE);
def("GET_EXTERNAL_POCKETS_MAX",&GET_EXTERNAL_POCKETS_MAX);
Expand Down
1 change: 1 addition & 0 deletions src/emc/rs274ngc/gcodemodule.cc
Expand Up @@ -498,6 +498,7 @@ void RIGID_TAP(int line_number,
Py_XDECREF(result);
}
double GET_EXTERNAL_MOTION_CONTROL_TOLERANCE() { return 0.1; }
double GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE() { return 0.1; }
double GET_EXTERNAL_PROBE_POSITION_X() { return _pos_x; }
double GET_EXTERNAL_PROBE_POSITION_Y() { return _pos_y; }
double GET_EXTERNAL_PROBE_POSITION_Z() { return _pos_z; }
Expand Down
2 changes: 1 addition & 1 deletion src/emc/rs274ngc/interp_base.hh
Expand Up @@ -27,7 +27,7 @@
/* Size of certain arrays */
#define ACTIVE_G_CODES 17
#define ACTIVE_M_CODES 10
#define ACTIVE_SETTINGS 3
#define ACTIVE_SETTINGS 5

class InterpBase : boost::noncopyable {
public:
Expand Down
163 changes: 109 additions & 54 deletions src/emc/rs274ngc/interp_convert.cc
Expand Up @@ -1561,11 +1561,13 @@ already in force.
*/

int Interp::convert_control_mode(int g_code, //!< g_code being executed (G_61, G61_1, || G_64)
double tolerance, //tolerance for the path following in G64
double naivecam_tolerance, //tolerance for the naivecam
setup_pointer settings) //!< pointer to machine settings
int Interp::convert_control_mode(
int g_code, // g_code being executed (G_61, G61_1, G_64)
double tolerance_in, // tolerance for the path following in G64
double naivecam_tolerance_in, // tolerance for the naivecam
setup_pointer settings) // pointer to machine settings
{
double tolerance, naivecam_tolerance;
CHKS((settings->cutter_comp_side),
(_("Cannot change control mode with cutter radius compensation on")));
if (g_code == G_61) {
Expand All @@ -1575,19 +1577,24 @@ int Interp::convert_control_mode(int g_code, //!< g_code being executed (G_6
SET_MOTION_CONTROL_MODE(CANON_EXACT_STOP, 0);
settings->control_mode = CANON_EXACT_STOP;
} else if (g_code == G_64) {
if (tolerance >= 0) {
SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, tolerance);
} else {
SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 0);
}
if (naivecam_tolerance >= 0) {
SET_NAIVECAM_TOLERANCE(naivecam_tolerance);
} else if (tolerance >= 0) {
SET_NAIVECAM_TOLERANCE(tolerance); // if no naivecam_tolerance specified use same for both
} else {
SET_NAIVECAM_TOLERANCE(0);
}
settings->control_mode = CANON_CONTINUOUS;
if (tolerance_in >= 0)
tolerance = tolerance_in;
else
tolerance = 0;
settings->control_mode = CANON_CONTINUOUS;
settings->tolerance = tolerance;
SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, tolerance);

if (naivecam_tolerance_in >= 0)
naivecam_tolerance = naivecam_tolerance_in;
else if (tolerance_in >= 0)
// if no naivecam_tolerance specified use same for both
naivecam_tolerance = tolerance_in;
else
naivecam_tolerance = 0;
settings->naivecam_tolerance = naivecam_tolerance;
SET_NAIVECAM_TOLERANCE(naivecam_tolerance);

} else
ERS(NCE_BUG_CODE_NOT_G61_G61_1_OR_G64);
return INTERP_OK;
Expand Down Expand Up @@ -2708,43 +2715,48 @@ int Interp::convert_length_units(int g_code, //!< g_code being executed (mus


/*
* given two double arrays representing interpreter settings as stored in
* _setup.active_settings, construct a G-code sequence to synchronize their state.
* given two int arrays and two double arrays representing interpreter
* settings as stored in _setup.active_g_codes and
* _setup.active_settings, construct a G-code sequence to synchronize
* their state.
*/
int Interp::gen_settings(double *current, double *saved, std::string &cmd)
int Interp::gen_settings(
int *int_current, int *int_saved, // G codes
double *float_current, double *float_saved, // S, F, other
std::string &cmd) // command buffer
{
int i;
int i, val;
int g64_changed = 0;
char buf[LINELEN];

// F, S
for (i = 0; i < ACTIVE_SETTINGS; i++) {
if (saved[i] != current[i]) {
if (float_saved[i] != float_current[i]) {
switch (i) {
case 0: break; // sequence_number - no point in restoring
case 1:
snprintf(buf,sizeof(buf)," F%.1f", saved[i]);
case GM_FIELD_FLOAT_LINE_NUMBER:
// sequence_number - no point in restoring
break;
case GM_FIELD_FLOAT_FEED:
snprintf(buf,sizeof(buf)," F%.1f", float_saved[i]);
cmd += buf;
break;
case 2:
snprintf(buf,sizeof(buf)," S%.0f", saved[i]);
case GM_FIELD_FLOAT_SPEED:
snprintf(buf,sizeof(buf)," S%.0f", float_saved[i]);
cmd += buf;
break;
case GM_FIELD_FLOAT_PATH_TOLERANCE:
case GM_FIELD_FLOAT_NAIVE_CAM_TOLERANCE:
// G64 special case; see below
g64_changed = 1;
break;
}
}
}
return INTERP_OK;
}


/*
* given two int arrays representing interpreter settings as stored in
* _setup.active_g_codes, construct a G-code sequence to synchronize their state.
*/
int Interp::gen_g_codes(int *current, int *saved, std::string &cmd)
{
int i, val;
char buf[LINELEN];
// G codes
for (i = 0; i < ACTIVE_G_CODES; i++) {
val = saved[i];
if (val != current[i]) {
val = int_saved[i];
if (val != int_current[i]) {

switch (i) {
case 0:
Expand All @@ -2768,7 +2780,6 @@ int Interp::gen_g_codes(int *current, int *saved, std::string &cmd)
case 8: // - coordinate system
case 9: // - tool offset (G43/G49)
case 10: // - retract mode
case 11: // - control mode
case 13: // - spindle mode
case 14: // - ijk distance mode
case 15: // - lathe diameter mode
Expand All @@ -2785,12 +2796,50 @@ int Interp::gen_g_codes(int *current, int *saved, std::string &cmd)
cmd += buf;
} else {
// so complain rather loudly
MSG("------ gen_g_codes BUG: index %d = -1!!\n",i);
MSG("------ gen_settings BUG: index %d = -1!!\n",i);
}
break;
case 11: // - control mode
// Special case, since G64 requires P and Q flags
// FIXME what about when P or Q changes, even though still G64?
if (val == G_64)
// G64 special case; see below
g64_changed = 1;
else if (val == G_61) {
snprintf(buf,sizeof(buf)," G61");
cmd += buf;
} else if (val == G_61_1) {
snprintf(buf,sizeof(buf)," G61.1");
cmd += buf;
} else
MSG("------ gen_settings BUG: index %d = -1!!\n",i);
break;
}
}
}


// Special case for restoring G64: `cmd` may contain a `G64` if
// current int settings is `G61` or if current float settings
// contain different `G64 P* Q*` args; in these cases, only add a
// single `G64` command
if ((int_saved[11] == G_64) && g64_changed) {
if (float_saved[GM_FIELD_FLOAT_PATH_TOLERANCE] < 0)
// No P, Q args
snprintf(buf,sizeof(buf)," G64");
else if (
float_saved[GM_FIELD_FLOAT_NAIVE_CAM_TOLERANCE] < 0)
// Only P arg
snprintf(buf,sizeof(buf)," G64 P%f",
float_saved[GM_FIELD_FLOAT_PATH_TOLERANCE]);
else // Both P, Q args
snprintf(
buf,sizeof(buf)," G64 P%f Q%f",
float_saved[GM_FIELD_FLOAT_PATH_TOLERANCE],
float_saved[GM_FIELD_FLOAT_NAIVE_CAM_TOLERANCE]);
cmd += buf;
}

return INTERP_OK;
}

Expand Down Expand Up @@ -2852,12 +2901,14 @@ int Interp::gen_restore_cmd(int *current_g,
StateTag const &saved,
std::string &cmd)
{
int res;
// A local copy of the saved settings, unpacked from a state tag
int saved_g[ACTIVE_G_CODES];
int saved_m[ACTIVE_M_CODES];
double saved_settings[ACTIVE_SETTINGS];

//Extract saved state to local vectors
// FIXME Use saved_settings to store state tag floats, incl. in
int res_unpack = active_modes(saved_g, saved_m, saved_settings, saved);
if (res_unpack != INTERP_OK) {
logStateTags("gen_restore_cmd() error %d: failed to unpack state tag",
Expand All @@ -2880,17 +2931,14 @@ int Interp::gen_restore_cmd(int *current_g,
cmd.c_str());
}

int res_settings = gen_settings(current_settings, saved_settings, cmd);
//M codes should not be restored during an abort
int res_m = 0;
int res_g = gen_g_codes(current_g, saved_g, cmd);

if (res_settings || res_m || res_g) {
logStateTags("gen_restore_cmd(): error restoring "
"settings (%d) or G codes (%d)",
res_settings, res_g);
if ((res = gen_settings(
current_g, saved_g, current_settings, saved_settings, cmd))) {
logStateTags("gen_restore_cmd(): error restoring settings (%d)",
res);
return INTERP_ERROR;
}
// M codes should not be restored during an abort with gen_m_codes()

return INTERP_OK;
}

Expand Down Expand Up @@ -2952,9 +3000,16 @@ int Interp::restore_settings(setup_pointer settings,
snprintf(buf,sizeof(buf), "G%d",settings->sub_context[from_level].saved_g_codes[5]/10);
CHKS(execute(buf) != INTERP_OK, _("M7x: restore_settings G20/G21 failed: '%s'"), cmd.c_str());
}
gen_settings((double *)settings->active_settings, (double *)settings->sub_context[from_level].saved_settings,cmd);
gen_m_codes((int *) settings->active_m_codes, (int *)settings->sub_context[from_level].saved_m_codes,cmd);
gen_g_codes((int *)settings->active_g_codes, (int *)settings->sub_context[from_level].saved_g_codes,cmd);
gen_settings(
(int *)settings->active_g_codes,
(int *)settings->sub_context[from_level].saved_g_codes,
(double *)settings->active_settings,
(double *)settings->sub_context[from_level].saved_settings,
cmd);
gen_m_codes(
(int *) settings->active_m_codes,
(int *)settings->sub_context[from_level].saved_m_codes,
cmd);

if (!cmd.empty()) {
// the sequence can be multiline, separated by nl
Expand Down
2 changes: 2 additions & 0 deletions src/emc/rs274ngc/interp_internal.hh
Expand Up @@ -681,6 +681,8 @@ struct setup

char blocktext[LINELEN]; // linetext downcased, white space gone
CANON_MOTION_MODE control_mode; // exact path or cutting mode
double tolerance; // G64 blending tolerance
double naivecam_tolerance; // G64 naive cam tolerance
int current_pocket; // carousel slot number of current tool
double current_x; // current X-axis position
double current_y; // current Y-axis position
Expand Down
19 changes: 14 additions & 5 deletions src/emc/rs274ngc/interp_write.cc
Expand Up @@ -114,6 +114,7 @@ int Interp::write_g_codes(block_pointer block, //!< pointer to a block of RS27
settings->tool_offset.a || settings->tool_offset.b || settings->tool_offset.c ||
settings->tool_offset.u || settings->tool_offset.v || settings->tool_offset.w) ? G_43 : G_49;
gez[10] = (settings->retract_mode == OLD_Z) ? G_98 : G_99;
// Three modes: G_64, G_61, G_61_1 or CANON_CONTINUOUS/EXACT_PATH/EXACT_STOP
gez[11] =
(settings->control_mode == CANON_CONTINUOUS) ? G_64 :
(settings->control_mode == CANON_EXACT_PATH) ? G_61 : G_61_1;
Expand Down Expand Up @@ -197,9 +198,11 @@ int Interp::write_settings(setup_pointer settings) //!< pointer to machine
double *vals;

vals = settings->active_settings;
vals[0] = settings->sequence_number; /* 0 sequence number */
vals[1] = settings->feed_rate; /* 1 feed rate */
vals[2] = settings->speed[0]; /* 2 spindle speed */
vals[0] = settings->sequence_number; /* 0 sequence number */
vals[1] = settings->feed_rate; /* 1 feed rate */
vals[2] = settings->speed; /* 2 spindle speed */
vals[3] = settings->tolerance; /* 3 blend tolerance */
vals[4] = settings->naivecam_tolerance; /* 4 naive CAM tolerance */

return INTERP_OK;
}
Expand Down Expand Up @@ -279,10 +282,16 @@ int Interp::write_state_tag(block_pointer block,
settings->tool_offset.w);
state.flags[GM_FLAG_RETRACT_OLDZ] =
(settings->retract_mode == OLD_Z);

state.flags[GM_FLAG_BLEND] =
(settings->control_mode == CANON_CONTINUOUS);
state.flags[GM_FLAG_EXACT_STOP] =
(settings->control_mode == CANON_EXACT_STOP);
state.fields_float[GM_FIELD_FLOAT_PATH_TOLERANCE] =
settings->tolerance;
state.fields_float[GM_FIELD_FLOAT_NAIVE_CAM_TOLERANCE] =
settings->naivecam_tolerance;

state.flags[GM_FLAG_CSS_MODE] =
(settings->spindle_mode == CONSTANT_RPM);
state.flags[GM_FLAG_IJK_ABS] =
Expand Down Expand Up @@ -312,8 +321,8 @@ int Interp::write_state_tag(block_pointer block,

state.flags[GM_FLAG_FEED_HOLD] = (settings->feed_hold);

state.feed = settings->feed_rate; /* 1 feed rate */
state.speed = settings->speed; /* 2 spindle speed */
state.fields_float[GM_FIELD_FLOAT_FEED] = settings->feed_rate;
state.fields_float[GM_FIELD_FLOAT_SPEED] = settings->speed;

return 0;
}
Expand Down

0 comments on commit 6bc4a2a

Please sign in to comment.